import math

try :    
    cmds.delete( dellist )
    dellist = []
except :
    dellist = []

def distance(a,b):    
    tot = 0
    for i in range(3):
        tot +=(a[i]-b[i])**2
    return math.sqrt(tot)

def getHich( tg ):    
    main = [ m for m in cmds.listRelatives( tg,c = 1) if 'main' in m]
    leg_grp = [ l for l in cmds.listRelatives( tg,c = 1) if 'leg_grp' in l]
    null_grp = [ l for l in cmds.listRelatives( main ,c = 1) if 'null' in l]
    legs = cmds.listRelatives( leg_grp[0],c = 1)
    return {'main':main , 'legs':legs , 'null':null_grp}
    
mode = 1
radL = 0
radR = 0

def antwalk( ant ):
    global radL
    global radR
    
    
    ant_main = ant['main'][0]
    ant_lfft_ctrl = [a for a in ant['legs'] if 'lfft' in a ][0]
    ant_lfmd_ctrl = [a for a in ant['legs'] if 'lfmd' in a ][0]
    ant_lfbk_ctrl = [a for a in ant['legs'] if 'lfbk' in a ][0]
    ant_rtft_ctrl = [a for a in ant['legs'] if 'rtft' in a ][0]
    ant_rtmd_ctrl = [a for a in ant['legs'] if 'rtmd' in a ][0]
    ant_rtbk_ctrl = [a for a in ant['legs'] if 'rtbk' in a ][0]
    
    ant_lfft_null = [a for a in ant['null'] if 'lfft' in a ][0]
    ant_lfmd_null = [a for a in ant['null'] if 'lfmd' in a ][0]
    ant_lfbk_null = [a for a in ant['null'] if 'lfbk' in a ][0]
    ant_rtft_null = [a for a in ant['null'] if 'rtft' in a ][0]
    ant_rtmd_null = [a for a in ant['null'] if 'rtmd' in a ][0]
    ant_rtbk_null = [a for a in ant['null'] if 'rtbk' in a ][0]

    # initial
    ct = cmds.currentTime( q=1 )
    if ct == 1 :        
        for x in ant['legs']:
            map( lambda s :cmds.setAttr( '%s.t' % x + s , 0 ) , 'xy' )

    mainX = cmds.getAttr( ant_main + '.tx' )
    mainX1 = cmds.getAttr( ant_main + '.tx' , t = ct-1 )
    mainT = cmds.getAttr( ant_main + '.t' , t = ct-1 )[0] 
    #dist2Main = mainX - mainX1
    dist2Main = 1 if cmds.getAttr( ant_main + '.worldMatrix') != \
                cmds.getAttr( ant_main + '.worldMatrix' , t= ct-1) \
                else 0
                

    upSpeed = 5.0
    liftH = .15
    
    ## Front
    dist_rtft = 0 if cmds.getAttr(ant_rtft_null+'.worldMatrix') == \
                    cmds.getAttr(ant_rtft_null+'.worldMatrix', t = ct-1 ) else 1                     
    if  not dist2Main and not max( 0 , math.sin( radR ) * liftH ) :
        pass
    else :
        if cmds.getAttr( ant_lfft_ctrl + '.ty') == 0 :
            cmds.setAttr( ant_rtft_ctrl + '.ty' , max( 0 , math.sin( radR ) * liftH ) )       
            radR = ( radR + math.pi / upSpeed  )
            
    if cmds.getAttr(ant_rtft_ctrl+'.ty') != 0 :        
        cmds.setAttr( ant_rtft_ctrl+'.tx' , mainT[0] )
        cmds.setAttr( ant_rtft_ctrl+'.tz' ,mainT[2] )

    dist_lfft = 0 if cmds.getAttr(ant_lfft_null+'.worldMatrix') == \
                    cmds.getAttr(ant_lfft_null+'.worldMatrix', t = ct-1 ) else 1 
    if not dist2Main and not max( 0 , math.sin( radL ) * liftH ) :
        pass
    else :
        if cmds.getAttr(ant_rtft_ctrl+'.ty') == 0 :
            cmds.setAttr( ant_lfft_ctrl+'.ty' , max( 0 , math.sin( radL ) * liftH ) )        
            radL = ( radL + math.pi / upSpeed )
            
    if cmds.getAttr(ant_lfft_ctrl+'.ty') != 0 :        
        cmds.setAttr( ant_lfft_ctrl+'.tx' , mainT[0] )
        cmds.setAttr( ant_lfft_ctrl+'.tz' , mainT[2] ) 
            
    ## Middle
    dist_rtmd = distance( cmds.getAttr(ant_rtmd_null+'.t')[0] ,
                         cmds.getAttr(ant_rtmd_null+'.t' , t = ct-1 )[0])
    if not dist2Main and not max( 0 , math.sin( radR ) * liftH ) :
        pass
    else :
        if cmds.getAttr(ant_lfmd_ctrl+'.ty') == 0 :
            cmds.setAttr( ant_rtmd_ctrl+'.ty' , max( 0 , math.sin( radR ) * liftH ) )        
            radR = ( radR + math.pi / upSpeed)
            
    if cmds.getAttr(ant_rtmd_ctrl+'.ty') != 0 :        
        cmds.setAttr( ant_rtmd_ctrl+'.tx' , mainT[0] )
        cmds.setAttr( ant_rtmd_ctrl+'.tz' , mainT[2] )             
           
    dist_lfmd = distance( cmds.getAttr(ant_lfmd_null+'.t')[0] ,
                         cmds.getAttr(ant_lfmd_null+'.t' , t = ct-1 )[0])
    if not dist2Main and not max( 0 , math.sin( radL ) * liftH ) :
        pass
    else :
        if cmds.getAttr(ant_rtmd_ctrl+'.ty') == 0 :        
            cmds.setAttr( ant_lfmd_ctrl+'.ty' , max( 0 , math.sin( radL ) * liftH ) )        
            radL = ( radL + math.pi / upSpeed )
    
    if cmds.getAttr(ant_lfmd_ctrl+'.ty') != 0 :        
        cmds.setAttr( ant_lfmd_ctrl+'.tx' , mainT[0] )
        cmds.setAttr( ant_lfmd_ctrl+'.tz' , mainT[2] )  
    
    ## Back  
    dist_lfbk = distance( cmds.getAttr(ant_lfbk_null+'.t')[0] ,
                         cmds.getAttr(ant_lfbk_null+'.t' , t = ct-1 )[0])
    if not dist2Main and not max( 0 , math.sin( radL ) * liftH ) ==0 :
        pass
    else :
        if cmds.getAttr(ant_rtbk_ctrl+'.ty') == 0 :        
            cmds.setAttr( ant_lfbk_ctrl+'.ty' , max( 0 , math.sin( radL ) * liftH ) )
            radL = ( radL + math.pi / upSpeed )
            
    if cmds.getAttr(ant_lfbk_ctrl+'.ty') != 0 :        
        cmds.setAttr( ant_lfbk_ctrl+'.tx' , mainT[0] )
        cmds.setAttr( ant_lfbk_ctrl+'.tz' , mainT[2] )
        
    dist_rtbk = distance( cmds.getAttr(ant_rtbk_null+'.t')[0] ,
                         cmds.getAttr(ant_rtbk_null+'.t' , t = ct-1 )[0])
    if not dist2Main and not max( 0 , math.sin( radR ) * liftH ) :
        pass
    else :
        if cmds.getAttr(ant_lfbk_ctrl+'.ty') == 0 :
            cmds.setAttr( ant_rtbk_ctrl+'.ty' , max( 0 , math.sin( radR ) *  liftH ) )
            radR = ( radR + math.pi / upSpeed)
            
    if cmds.getAttr(ant_rtbk_ctrl+'.ty') != 0 :        
        cmds.setAttr( ant_rtbk_ctrl+'.tx' , mainT[0] )
        cmds.setAttr( ant_rtbk_ctrl+'.tz' , mainT[2] )        
        
    if  dist2Main == 0  :
        for x in ant['legs']:
            cmds.setAttr( '%s.ty' % x  , 0 )